/*
 * Copyright 2025 The Authors
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *      http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 *
 *@file    : triangulate_factor.cc
 *@brief   : triangulate factor
 *@authors : zhanglei
 *@version : v1.0
 *@data    : 2025/9/13
 *
 */

#include "optimizer/triangulate_factor.h"

namespace optimizer {

TriangulateFactor::TriangulateFactor(const Eigen::Matrix3d& intrinsic_matrix, const Eigen::Matrix4d& extrinsic_matrix) : intrinsic_matrix_(intrinsic_matrix), extrinsic_matrix_(extrinsic_matrix) {

}

bool TriangulateFactor::OptimizeWorldPoints(
        const std::vector<Eigen::Vector2d>& normalized_points, 
        const std::vector<Eigen::Matrix4d>& world_poses,            
        Eigen::Vector3d& world_point) {

    // 检查输入数据有效性
    if (normalized_points.empty()) {
        return false;
    }

    // 初始值
    double initial_point[3] = {
        world_point.x(), world_point.y(), world_point.z()
    };

    // 创建问题
    ceres::Problem problem;
    CameraIntrinsics intrinsics;  // 使用默认归一化内参

    // 添加残差项
    for (size_t i = 0; i < normalized_points.size(); ++i) {
        const auto& pt = normalized_points[i];
        // for(uint32_t pose_idx = 0; pose_idx < world_poses.size(); ++pose_idx) {
            const auto& pose_matrix = world_poses[i];
            CameraPose pose;
            pose.rotation = Eigen::Quaterniond(pose_matrix.block<3,3>(0,0));
            pose.translation = pose_matrix.block<3,1>(0,3);
            ceres::CostFunction* cost_function = ReprojectionErrorCostFunctor::Create(
                pt.x(), pt.y(), pose, extrinsic_matrix_, intrinsics
            );
        
            problem.AddResidualBlock(
                cost_function,
                new ceres::HuberLoss(1.0),  // 使用Huber损失函数减少异常值影响
                initial_point
            );
        // }
    }

    // 配置求解器
    ceres::Solver::Options options;
    options.linear_solver_type = ceres::DENSE_QR;  // 适用于小规模问题
    options.minimizer_progress_to_stdout = true;   // 输出优化过程
    options.max_num_iterations = 100;              // 最大迭代次数

    // 运行优化
    ceres::Solver::Summary summary;
    ceres::Solve(options, &problem, &summary);
    
    // 输出优化结果
    LOG(INFO) << summary.BriefReport() << std::endl;
    
    // 更新世界点
    world_point.x() = initial_point[0];
    world_point.y() = initial_point[1];
    world_point.z() = initial_point[2];
    
    return summary.IsSolutionUsable();
}

}
